#include <boost/graph/adjacency_list.hpp>
#include <boost/graph/dijkstra_shortest_paths.hpp>
#include <map>
#include <cmath>
#include <vector>
#include <limits>
#include <algorithm>

struct Node
{
    float x, y, z;

    Node() : x(0), y(0), z(0) {}
    Node(float x, float y, float z) : x(x), y(y), z(z) {}

    // 使用epsilon的比较操作
    bool operator<(const Node &other) const
    {
        constexpr float EPSILON = 1e-6f;
        if (std::abs(x - other.x) > EPSILON)
            return x < other.x;
        if (std::abs(y - other.y) > EPSILON)
            return y < other.y;
        return z < other.z;
    }

    bool operator==(const Node &other) const
    {
        constexpr float EPSILON = 1e-6f;
        return std::abs(x - other.x) < EPSILON &&
               std::abs(y - other.y) < EPSILON &&
               std::abs(z - other.z) < EPSILON;
    }

    Node operator-(const Node &other) const
    {
        return Node(x - other.x, y - other.y, z - other.z);
    }
};

struct NodeCompare
{
    bool operator()(const Node &a, const Node &b) const { return a < b; }
};

typedef boost::adjacency_list<
    boost::vecS, boost::vecS, boost::undirectedS,
    Node, boost::property<boost::edge_weight_t, float>>
    BGLGraph;
typedef boost::graph_traits<BGLGraph>::vertex_descriptor VertexDesc;
typedef boost::graph_traits<BGLGraph>::edge_descriptor EdgeDesc;

class Graph
{

    BGLGraph bgl_graph;

    // 辅助结构：节点坐标到顶点描述符的映射（唯一性检查）
    std::map<Node, VertexDesc, NodeCompare> node_map;

    // 辅助结构：顶点索引到顶点描述符的映射（保持原有索引接口）
    std::vector<VertexDesc> vertex_indices;

    // 自定义比较函数（使用epsilon比较）

public:
    void clear()
    {
        bgl_graph = BGLGraph();
        node_map.clear();              // 清空节点坐标到顶点描述符的映射
        vertex_indices.clear();        // 清空顶点索引到顶点描述符的映射
    }
    void copy_from(const Graph &other)
    {
        if (this == &other)
            return; // 自赋值检查

        clear(); // 清空当前图

        // 1. 复制所有节点
        for (const auto &v_desc : other.vertex_indices)
        {
            const Node &node = other.bgl_graph[v_desc];
            add_node(node.x, node.y, node.z); // 添加节点（自动处理唯一性）
        }

        // 2. 复制所有边
        typedef boost::graph_traits<BGLGraph>::edge_iterator EdgeIter;
        std::pair<EdgeIter, EdgeIter> edge_pair = boost::edges(other.bgl_graph);
        for (EdgeIter e_iter = edge_pair.first; e_iter != edge_pair.second; ++e_iter)
        {
            EdgeDesc e = *e_iter;
            VertexDesc u_other = boost::source(e, other.bgl_graph);
            VertexDesc v_other = boost::target(e, other.bgl_graph);

            const Node &u_node = other.bgl_graph[u_other];
            const Node &v_node = other.bgl_graph[v_other];

            // 查找当前图中对应的顶点描述符
            auto u_it = node_map.find(u_node);
            auto v_it = node_map.find(v_node);

            if (u_it != node_map.end() && v_it != node_map.end())
            {
                VertexDesc u_this = u_it->second;
                VertexDesc v_this = v_it->second;

                // 获取边的权重并添加边
                float weight = boost::get(boost::edge_weight, other.bgl_graph, e);
                boost::add_edge(u_this, v_this, weight, bgl_graph);
            }
        }
    }

    // 添加节点
    void add_node(float x, float y, float z)
    {
        Node new_node(x, y, z);
        auto it = node_map.find(new_node);
        if (it != node_map.end())
            return;

        VertexDesc v = boost::add_vertex(new_node, bgl_graph);
        node_map[new_node] = v;
        vertex_indices.push_back(v); // 维护索引映射
    }

    // 添加边
    void add_edge(size_t from, size_t to)
    {
        if (from == to || from >= vertex_indices.size() || to >= vertex_indices.size())
            return;

        VertexDesc u = vertex_indices[from];
        VertexDesc v = vertex_indices[to];

        // Check if the edge already exists
        EdgeDesc e;
        bool exists;
        boost::tie(e, exists) = boost::edge(u, v, bgl_graph);
        if (exists)
            return;

        // Compute the edge weight
        float weight = distance(from, to);

        // Add edge with weight
        boost::add_edge(u, v, weight, bgl_graph); // Requires edge weight property
    }

    // 删除节点
    bool delete_node(size_t index)
    {
        if (index >= vertex_indices.size())
            return false;

        VertexDesc v = vertex_indices[index];
        Node removed_node = bgl_graph[v]; // 获取节点信息

        boost::clear_vertex(v, bgl_graph);  // 移除所有关联边
        boost::remove_vertex(v, bgl_graph); // 删除顶点

        // 更新辅助结构
        vertex_indices.erase(vertex_indices.begin() + index);
        node_map.erase(removed_node); // 同步删除映射
        return true;
    }

    // 删除边（根据顶点索引）
    bool delete_edge(size_t from, size_t to)
    {
        if (from == to || from >= vertex_indices.size() || to >= vertex_indices.size())
            return false;

        VertexDesc u = vertex_indices[from];
        VertexDesc v = vertex_indices[to];
        boost::remove_edge(u, v, bgl_graph);
        return true;
    }

    // 获取节点数量
    size_t num_nodes() const
    {
        return vertex_indices.size(); // 保持与辅助结构一致
    }

    // 获取边数量
    size_t num_edges() const
    {
        return boost::num_edges(bgl_graph);
    }

    // 获取节点属性
    const Node &get_node(size_t index) const
    {
        static Node empty;
        if (index >= vertex_indices.size())
            return empty;

        return bgl_graph[vertex_indices[index]];
    }

    // 计算两点间距离
    float distance(size_t from, size_t to) const
    {
        const Node &a = get_node(from);
        const Node &b = get_node(to);
        return std::sqrt(distance_squared(a, b));
    }

private:
    // 距离平方计算
    inline float distance_squared(const Node &a, const Node &b) const
    {
        float dx = a.x - b.x;
        float dy = a.y - b.y;
        float dz = a.z - b.z;
        return dx * dx + dy * dy + dz * dz;
    }

public:
    // 寻找最近节点
    size_t find_nearest_node(float x, float y, float z) const
    {
        if (vertex_indices.empty())
            return std::numeric_limits<size_t>::max(); // 返回无效索引

        Node target(x, y, z);
        float min_dist_sq = std::numeric_limits<float>::max();
        size_t nearest_idx = 0;

        // 遍历所有节点
        for (size_t i = 0; i < vertex_indices.size(); ++i)
        {
            const Node &node = bgl_graph[vertex_indices[i]];
            float dist_sq = distance_squared(node, target);
            if (dist_sq < min_dist_sq)
            {
                min_dist_sq = dist_sq;
                nearest_idx = i;
            }
        }

        return nearest_idx;
    }
    std::vector<Node> find_way(size_t from, size_t to) const
    {
        std::vector<Node> path;

        if (from >= vertex_indices.size() || to >= vertex_indices.size())
            return path; // Invalid indices

        VertexDesc start = vertex_indices[from];
        VertexDesc goal = vertex_indices[to];

        // Number of vertices in the graph
        size_t num_vertices = boost::num_vertices(bgl_graph);

        // Property maps for distances and predecessors
        std::vector<float> distances(num_vertices, std::numeric_limits<float>::infinity());
        std::vector<VertexDesc> predecessors(num_vertices);

        // Run Dijkstra's algorithm
        boost::dijkstra_shortest_paths(
            bgl_graph, start,
            boost::predecessor_map(predecessors.data())
                .distance_map(distances.data())
                .weight_map(boost::get(boost::edge_weight, bgl_graph)));

        // If no path exists
        if (distances[goal] == std::numeric_limits<float>::infinity())
            return path;

        // Reconstruct the path
        for (VertexDesc v = goal; v != start; v = predecessors[v])
        {
            path.push_back(bgl_graph[v]);
        }
        path.push_back(bgl_graph[start]);

        // Reverse the path to get it from start to goal
        std::reverse(path.begin(), path.end());

        return path;
    }
};